from math import pi
from time import sleep, sleep_ms
from mpython import *
from machine import I2C,Pin


import struct


REG_SERVO = 0x1
REG_MOTOR = 0x2
REG_PIDRUN = 0x3
REG_PIDSAV = 0x4
REG_PIDLOD = 0x5
REG_DUALRUN = 0x6
REG_VOLTAG = 0x8
REG_PIDRESET = 0x9

MALL = 0
M1A = 1
M1B = 2
M2A = 3
M2B = 4

S1 = 1
S2 = 2
S3 = 3
S4 = 4

Kp = 1
Ki = 2
Kd = 3
POSITION = 4
SPEED = 5
PULSE = 6
GEARRATIO = 7
LOOPMS = 8
Kp_hold = 9
Ki_hold = 10
Kd_hold = 11
RAMPDOWN = 12

MODE_IDLE   = 0x0
MODE_SPEED  = 0x1
MODE_HOLD   = 0x2
MODE_RUNPOS = 0x4 # there is some end position
MODE_ABSOLU = 0x8
MODE_DELAY  = 0x10
MODE_STUCK  = 0x20
MODE_RAMPDN = 0x40
MODE_TOSTOP = 0x80

MODE_RUN    = 0x1
MODE_TURN   = 0x2

# audio instance at GPIO 26
#audio = Audio(26)

# reg read map
# 60~70~80 pid1 pid2 piddual
# reg write map
# A0~B0~C0 pid1 pid2 piddual

addr = 29

## meowbit i2c
# from meowbit import i2c
# def _i2cWrite(reg, b):
#   i2c.mem_write(b, addr, reg)

# def _i2cRead(reg, n):
#   return i2c.mem_read(n, addr, reg)

## esp32 i2c
#from future import i2c
def _i2cWrite(reg, b):
  try:
    i2c.writeto_mem(addr, reg, b)
  except Exception as e:
    if "ENODEV" in str(e):
      screen.clear()
      screen.textCh('未检测到方糖盒子!',x=80-8*12/2, y=58, color=(255,0,0))
      screen.refresh()
    raise e

def _i2cRead(reg, n):
  try:
    return i2c.readfrom_mem(addr, reg, n)
  except Exception as e:
    if "ENODEV" in str(e):
      screen.clear()
      screen.textCh('未检测到方糖盒子!',x=80-8*12/2, y=58, color=(255,0,0))
      screen.refresh()  
    raise e 
  


def servo(index, degree=90, us=None):
  # (0)500us ~ (180)2500us
  if not us:
    us = int(degree*200/36+500) #2kg
  b = struct.pack("<BH", index, us)
  _i2cWrite(REG_SERVO, b)
  

def motor(index, speed):
  if index==0:
    for i in range(1,5):
      b = struct.pack("<BH", i, speed)
      _i2cWrite(REG_MOTOR, b)
  else:
    b = struct.pack("<BH", index, speed)
    _i2cWrite(REG_MOTOR, b)


def stop(index):
  if index==0:
    for i in range(1,5):
      b = struct.pack("<BH", i, 0)
      _i2cWrite(REG_MOTOR, b)
  else:
    b = struct.pack("<BH", index, 0)
    _i2cWrite(REG_MOTOR, b)


def voltage():
  t = _i2cRead(REG_VOLTAG, 4)
  return round(struct.unpack('f', t)[0], 2)

class EMotor:
  def __init__(self, index):
    self.index = index
    self.reset()
    self._mode = 0

  def reset(self):
    _i2cWrite(REG_PIDRESET, bytearray([self.index, 0]))

  # 以speed(圈/秒) 运动, param2为第二参数, 四字节浮点
  # 第二个字节为编码电机MODE
  def _pidRun(self, mode, speed, param2, wait):
    b = struct.pack("<BBff",self.index, mode, speed, param2)
    _i2cWrite(REG_PIDRUN, b)
    if wait:
      _reg = 0
      if self.index == 1:
        _reg = 0x60
      else:
        _reg = 0x70
      while mode:
        t = _i2cRead(_reg, 4)
        mode = t[0]
        if mode & MODE_STUCK:
          _i2cWrite(REG_PIDRUN, b)
          self.reset()
          raise Exception("Motor Stuck")
        sleep_ms(200)

  def _pidMode(self, mode, param1, param2):
    b = struct.pack("<BBLL",self.index, mode, param1, param2)
    _i2cWrite(REG_PIDRUN, b)

  def _pidRead(self, key):
    # reg 60~70 pid1 pid2
    reg = 0
    if self.index == 1:
      reg = 0x60
    else:
      reg = 0x70
    reg += (key & 0xf)
    t = _i2cRead(reg, 4)
    return struct.unpack('f', t)[0]

  def _pidSet(self, key, value):
    # reg A0~B0
    reg = 0
    if self.index == 1:
      reg = 0xA0
    else:
      reg = 0xB0
    reg += (key & 0xf)
    b = struct.pack("f", value)
    _i2cWrite(reg, b)

  def pidSave(self, reset=0):
    _i2cWrite(REG_PIDSAV, bytearray([reset]))

  def run_to_degree(self, degree, rspeed=120, wait=True):
    # speed to RPSec, degree to round
    degree /= 360.0
    self.run_for_rotations(degree, rspeed, wait)

  def run_for_degrees(self, degree, rspeed=120, wait=True):
    self.run_to_degree(degree, rspeed, wait)

  def run_for_rotations(self, round, rspeed=120, wait=True):
    if rspeed > 300:
        rspeed = 300    
    elif rspeed < -300:
        rspeed = -300
    speed = rspeed / 60.0
    self._pidRun(MODE_SPEED | MODE_RUNPOS, speed, round, wait)

  def run_for_seconds(self, sec, rspeed=120, wait=True):
    if rspeed > 300:
        rspeed = 300    
    elif rspeed < -300:
        rspeed = -300
    speed = rspeed / 60.0
    self._pidRun(MODE_SPEED | MODE_DELAY, speed, sec*1000, wait)

  def run(self, rspeed=120):
    if rspeed > 300:
        rspeed = 300    
    elif rspeed < -300:
        rspeed = -300
    speed = rspeed / 60.0
    self._pidRun(MODE_SPEED, speed, 0, False)

  def stop(self):
    self._pidRun(MODE_ABSOLU, 0, 0, False)

  def get_rspeed(self):
    f = self._pidRead(SPEED)
    # round per second to RPM
    return f*60

  # def get_position(self):
  #   return self._pidRead(POSITION)

  def get_degree(self):
    return self._pidRead(POSITION) * 360.0

  # def set_position(self, degree):
  #   return self._pidSet(POSITION, degree/360.0)


  # diameter车轮直径cm, vspeed 前进速度 cm/s  
  def dualRun_for_vspeed(self,diameter,vspeed):
      rspeed = vspeed / diameter * 60
      self.run(rspeed)


class EMotorDual:
  # in CM
  _R = 6.0
  _W = 12.0
  _Setup = 1
  _mode = 0
  _compansate = 1

  def __init__(self, diameter=6, width=12, left=2, right=1, dir=1):
    self._W = width
    self._R = diameter
    self._Setup = 0
    self._dir = dir
    # 左右马达互换
    if left == 2 and right == 1:
      self._Setup |= 0x1
    # 逆向前进
    if dir == -1:
      self._Setup |= 0x2
    self.reset()

  def reset(self):
    _i2cWrite(REG_PIDRESET, bytearray([3, self._Setup]))
  
  def set_diameter(self, d):
    self._R = d

  def set_width(self, d):
    self._Diameter = d
  
  def _pidRun(self, mode, rspeed, Wspeed, rnd, wait=True):
    # print("dpid", rspeed, Wspeed, rnd)
    b = struct.pack("<Bfff", mode, rspeed, Wspeed, rnd)
    _i2cWrite(REG_DUALRUN, b)
    if wait:
      _reg = 0x80
      while mode:
        t = _i2cRead(_reg, 4)
        mode = t[0]
        if mode & MODE_STUCK:
          self.reset()
          raise Exception("Motor Stuck")
        sleep_ms(200)
      
  # 直线运动: cm/s, 运动距离 CM
  def run(self, vspeed, distance=20, wait=True):
    self._mode = MODE_RUN
    # round in main asix pos  ,rnd直行时间
    rnd = distance/(pi*self._R)
    #print(rnd)
    rspeed = vspeed/(pi*self._R) # in round/s
    #print(rspeed)
    self._pidRun(self._mode, rspeed, 0, rnd, wait)

  # 转向运动: 旋转速度: degree/s, 旋转角度, 前进速度: cm/s, 
  def drive(self, steer=180, degree=180, speed=0, wait=True):
    self._mode = MODE_TURN
    speed = speed/(pi*self._R) # in round/s
    diff = steer*self._W/self._R/360 # wheel difference
    rnd = 2*(degree/steer)*diff
    # diff为两轮差速, 需要x2
    self._pidRun(self._mode, speed, 2*diff, rnd, wait)

  def _pidRead(self, key):
    reg = 0x80
    reg += (key & 0xf)
    t = _i2cRead(reg, 4)
    return struct.unpack('f', t)[0]

  def _pidSet(self, key, value):
    reg = 0xC0
    reg += (key & 0xf)
    b = struct.pack("f", value)
    _i2cWrite(reg, b)
   

class IntercomSugarBox():
    
    def __init__(self):
        self.BDADDR = b'\xff\xff\xff\xff\xff\xff'
        self.e = espnow.ESPNow()
        self.e.init()
        self.a = Audio(26)
        self.icontent = b''
        self.sendstat = True
        self.e.on_recv(self.rxcb)
        self.e.on_send(self.secb)
        self.playmsg = False

    def play(self):
        if self.playmsg:
            self.a.playbuf(self.icontent)
            self.icontent = b''
            self.playmsg = False
        sleep_ms(100)


    def rxcb(self,mac,msg):
        if 'MsgReceiveOK' in msg:
            self.playmsg = True
        else:
            self.icontent = self.icontent + msg

    def setchannel(self,ch):
        try:
            self.e.add_peer(self.BDADDR)
        except:
            pass
        if ch < 14 and ch > 0:
            self.e.channel(ch)
        else:
            raise Exception('invalid value')


    def intercomSend(self,sec):
        length = int(16000*sec)
        samples = bytearray(length*2)
        print('-->talk')
        ret = self.a.mic.readinto(samples)
        ss = struct.unpack('h'*length, samples)
        b = int(32768/max(max(ss), abs(min(ss))))

        tmp = []
        for n in range(0, len(ss), 2):
            tmp.append(int((ss[n]*b+32768)/256))
        tmp = bytes(tmp)
        size = len(tmp)
        print(size)
        for i in range(0,size,250):
            # print(i)
            retry = 0
            self.e.send(self.BDADDR,tmp[i:i+250])
            self.sendstat = True
            while self.sendstat:  # 异常处理稍后再做
                retry += 1
                sleep_ms(20)
        self.e.send(self.BDADDR,b'MsgReceiveOK')

    def secb(self,*args):
        self.sendstat = False